{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "4e1f0bb7",
   "metadata": {},
   "source": [
    "## 读取数据"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "0df90ff0",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-06-05T13:45:05.111416Z",
     "start_time": "2023-06-05T13:45:04.346512Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import copy\n",
    "import time\n",
    "import os\n",
    "import sys\n",
    "import argparse"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "b44a87b8",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-06-05T13:45:05.965769Z",
     "start_time": "2023-06-05T13:45:05.947457Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['/home/he/anaconda3/envs/xgbcom/lib/python3.7/site-packages/ipykernel_launcher.py', '-f', '/home/he/.local/share/jupyter/runtime/kernel-a962aecc-57cd-4f32-bd13-58a3676cec5f.json']\n"
     ]
    }
   ],
   "source": [
    "print(sys.argv)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "9d8d4281",
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw_registration_result(source, target, transformation):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    target_temp = copy.deepcopy(target)\n",
    "    source_temp.paint_uniform_color([1, 0.706, 0])\n",
    "    target_temp.paint_uniform_color([0, 0.651, 0.929])\n",
    "    source_temp.transform(transformation)\n",
    "    o3d.visualization.draw_geometries([source_temp, target_temp])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "56cb73df",
   "metadata": {},
   "outputs": [],
   "source": [
    "def preprocess_point_cloud(pcd, voxel_size):\n",
    "    pcd_down = pcd.voxel_down_sample(voxel_size)\n",
    "    pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,max_nn=30))\n",
    "    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(pcd_down,\n",
    "                                            o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,\n",
    "                                             max_nn=100))\n",
    "    return (pcd_down, pcd_fpfh)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "bbe653da",
   "metadata": {},
   "outputs": [],
   "source": [
    "class dair_data:\n",
    "    def __init__(self,path):\n",
    "        filename=os.listdir(path)\n",
    "        self.paths=[path+'/'+x for x in filename]    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "1417072d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'../VIC/vehicle-side/cooperative-vehicle-infrastructure-vehicle-side-velodyne/002238.pcd'"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#pcd_data = o3d.data.DemoICPPointClouds()\n",
    "inf_data=dair_data('../VIC/infrastructure-side/cooperative-vehicle-infrastructure-infrastructure-side-velodyne')\n",
    "print(inf_data.paths[0])\n",
    "veh_data=dair_data('../VIC/vehicle-side/cooperative-vehicle-infrastructure-vehicle-side-velodyne')\n",
    "print(veh_data.paths[0])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "a640775f",
   "metadata": {},
   "outputs": [],
   "source": [
    "pcd_data = o3d.data.DemoICPPointClouds()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "074ab542",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-06-05T13:46:16.372805Z",
     "start_time": "2023-06-05T13:45:15.018060Z"
    }
   },
   "outputs": [],
   "source": [
    "parser = argparse.ArgumentParser('Global point cloud registration example with RANSAC')\n",
    "parser.add_argument('src',\n",
    "                    type=str,\n",
    "                    default=pcd_data.paths[0],\n",
    "                    nargs='?',\n",
    "                    help='path to src point cloud')\n",
    "parser.add_argument('dst',\n",
    "                    type=str,\n",
    "                    default=pcd_data.paths[1],\n",
    "                    nargs='?',\n",
    "                    help='path to dst point cloud')\n",
    "parser.add_argument('--voxel_size',\n",
    "                    type=float,\n",
    "                    default=0.05,\n",
    "                    help='voxel size in meter used to downsample inputs')\n",
    "parser.add_argument(\n",
    "    '--distance_multiplier',\n",
    "    type=float,\n",
    "    default=1.5,\n",
    "    help='multipler used to compute distance threshold'\n",
    "    'between correspondences.'\n",
    "    'Threshold is computed by voxel_size * distance_multiplier.')\n",
    "parser.add_argument('--max_iterations',\n",
    "                    type=int,\n",
    "                    default=64,\n",
    "                    help='number of max FGR iterations')\n",
    "parser.add_argument(\n",
    "    '--max_tuples',\n",
    "    type=int,\n",
    "    default=1000,\n",
    "    help='max number of accepted tuples for correspondence filtering')\n",
    "\n",
    "args = parser.parse_args(args=[])\n",
    "\n",
    "\n",
    "\n",
    "#args=parser.parse_known_args()[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "id": "14d29726",
   "metadata": {},
   "outputs": [],
   "source": [
    "args.src=inf_data.paths[0]\n",
    "args.dst=veh_data.paths[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "4645959b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-06-05T13:46:22.673768Z",
     "start_time": "2023-06-05T13:46:22.602316Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[Open3D DEBUG] Format auto File ../VIC/infrastructure-side/cooperative-vehicle-infrastructure-infrastructure-side-velodyne/002238.pcd\n",
      "[Open3D DEBUG] PCD header indicates 4 fields, 16 bytes per point, and 65668 points in total.\n",
      "[Open3D DEBUG] x, F, 4, 1, 0\n",
      "[Open3D DEBUG] y, F, 4, 1, 4\n",
      "[Open3D DEBUG] z, F, 4, 1, 8\n",
      "[Open3D DEBUG] intensity, F, 4, 1, 12\n",
      "[Open3D DEBUG] Compression method is 2.\n",
      "[Open3D DEBUG] Points: yes;  normals: no;  colors: no\n",
      "[Open3D DEBUG] PCD data with 931465 compressed size, and 1050688 uncompressed size.\n",
      "[Open3D DEBUG] Read geometry::PointCloud: 65668 vertices.\n",
      "[Open3D DEBUG] Format auto File ../VIC/vehicle-side/cooperative-vehicle-infrastructure-vehicle-side-velodyne/002238.pcd\n",
      "[Open3D DEBUG] PCD header indicates 5 fields, 21 bytes per point, and 62583 points in total.\n",
      "[Open3D DEBUG] x, F, 4, 1, 0\n",
      "[Open3D DEBUG] y, F, 4, 1, 4\n",
      "[Open3D DEBUG] z, F, 4, 1, 8\n",
      "[Open3D DEBUG] intensity, U, 1, 1, 12\n",
      "[Open3D DEBUG] timestamp, F, 8, 1, 13\n",
      "[Open3D DEBUG] Compression method is 2.\n",
      "[Open3D DEBUG] Points: yes;  normals: no;  colors: no\n",
      "[Open3D DEBUG] PCD data with 1059070 compressed size, and 1314243 uncompressed size.\n",
      "[Open3D DEBUG] Read geometry::PointCloud: 62583 vertices.\n"
     ]
    }
   ],
   "source": [
    "src = o3d.io.read_point_cloud(args.src)\n",
    "dst = o3d.io.read_point_cloud(args.dst)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "f9b692a5",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-06-05T13:46:25.203562Z",
     "start_time": "2023-06-05T13:46:25.184117Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "PointCloud with 65668 points."
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "src"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "70a704fb",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-06-05T13:46:26.258118Z",
     "start_time": "2023-06-05T13:46:26.247595Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "PointCloud with 62583 points."
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dst"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "id": "ebc351ea",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "3"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "src.dimension()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5036507c",
   "metadata": {},
   "source": [
    "## 点云剪裁"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "0b1ab248",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'/home/he/open3d_data/extract/DemoCropPointCloud/cropped.json'"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "demo_crop_data.cropped_json_path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "id": "74f25ee8",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-06-05T13:50:22.800351Z",
     "start_time": "2023-06-05T13:50:09.520697Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Load a polygon volume and use it to crop the original point cloud\n",
      "[Open3D DEBUG] Format auto File /home/he/open3d_data/extract/DemoCropPointCloud/fragment.ply\n",
      "[Open3D DEBUG] Read geometry::PointCloud: 196133 vertices.\n",
      "[Open3D DEBUG] Add geometry and update bounding box to [(0.5508, 0.8320, 0.5586) - (3.9608, 2.4249, 2.5537)]\n"
     ]
    }
   ],
   "source": [
    "print(\"Load a polygon volume and use it to crop the original point cloud\")\n",
    "demo_crop_data = o3d.data.DemoCropPointCloud()\n",
    "pcd = o3d.io.read_point_cloud(demo_crop_data.point_cloud_path)\n",
    "#vol = o3d.visualization.read_selection_polygon_volume('/home/he/open3d_data/extract/DemoCropPointCloud/croppedtest.json')\n",
    "#chair = vol.crop_point_cloud(pcd)\n",
    "chair=src\n",
    "o3d.visualization.draw_geometries([chair],\n",
    "                                  zoom=0.7,\n",
    "                                  front=[0.5439, -0.2333, -0.8060],\n",
    "                                  lookat=[2.4615, 2.1331, 1.338],\n",
    "                                  up=[-0.1781, -0.9708, 0.1608])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "id": "1464ed88",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Load a polygon volume and use it to crop the original point cloud\n",
      "[Open3D DEBUG] Pointcloud down sampled from 198835 points to 17495 points.\n",
      "[Open3D DEBUG] Add geometry and update bounding box to [(0.5508, 0.8320, 0.5586) - (3.9608, 2.4249, 2.5537)]\n"
     ]
    }
   ],
   "source": [
    "print(\"Load a polygon volume and use it to crop the original point cloud\")\n",
    "#demo_crop_data = o3d.data.DemoCropPointCloud()\n",
    "#pcd = o3d.io.read_point_cloud(demo_crop_data.point_cloud_path)\n",
    "vol = o3d.visualization.read_selection_polygon_volume('/home/he/open3d_data/extract/DemoCropPointCloud/croppedtest.json')\n",
    "chair = vol.crop_point_cloud(src)\n",
    "o3d.visualization.draw_geometries([src],\n",
    "                                  zoom=0.7,\n",
    "                                  front=[0.5439, -0.2333, -0.8060],\n",
    "                                  lookat=[2.4615, 2.1331, 1.338],\n",
    "                                  up=[-0.1781, -0.9708, 0.1608])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "66f1aa0e",
   "metadata": {},
   "source": [
    "## 预处理"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "id": "81db897b",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reading inputs\n",
      "[Open3D DEBUG] Format auto File ../VIC/infrastructure-side/cooperative-vehicle-infrastructure-infrastructure-side-velodyne/002238.pcd\n",
      "[Open3D DEBUG] PCD header indicates 4 fields, 16 bytes per point, and 65668 points in total.\n",
      "[Open3D DEBUG] x, F, 4, 1, 0\n",
      "[Open3D DEBUG] y, F, 4, 1, 4\n",
      "[Open3D DEBUG] z, F, 4, 1, 8\n",
      "[Open3D DEBUG] intensity, F, 4, 1, 12\n",
      "[Open3D DEBUG] Compression method is 2.\n",
      "[Open3D DEBUG] Points: yes;  normals: no;  colors: no\n",
      "[Open3D DEBUG] PCD data with 931465 compressed size, and 1050688 uncompressed size.\n",
      "[Open3D DEBUG] Read geometry::PointCloud: 65668 vertices.\n",
      "[Open3D DEBUG] Format auto File ../VIC/vehicle-side/cooperative-vehicle-infrastructure-vehicle-side-velodyne/002238.pcd\n",
      "[Open3D DEBUG] PCD header indicates 5 fields, 21 bytes per point, and 62583 points in total.\n",
      "[Open3D DEBUG] x, F, 4, 1, 0\n",
      "[Open3D DEBUG] y, F, 4, 1, 4\n",
      "[Open3D DEBUG] z, F, 4, 1, 8\n",
      "[Open3D DEBUG] intensity, U, 1, 1, 12\n",
      "[Open3D DEBUG] timestamp, F, 8, 1, 13\n",
      "[Open3D DEBUG] Compression method is 2.\n",
      "[Open3D DEBUG] Points: yes;  normals: no;  colors: no\n",
      "[Open3D DEBUG] PCD data with 1059070 compressed size, and 1314243 uncompressed size.\n",
      "[Open3D DEBUG] Read geometry::PointCloud: 62583 vertices.\n",
      "Downsampling inputs\n",
      "[Open3D DEBUG] Pointcloud down sampled from 65668 points to 64088 points.\n",
      "[Open3D DEBUG] Pointcloud down sampled from 62583 points to 58182 points.\n"
     ]
    }
   ],
   "source": [
    "voxel_size = args.voxel_size\n",
    "distance_threshold = args.distance_multiplier * voxel_size\n",
    "\n",
    "o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)\n",
    "\n",
    "print('Reading inputs')\n",
    "src = o3d.io.read_point_cloud(args.src)\n",
    "dst = o3d.io.read_point_cloud(args.dst)\n",
    "\n",
    "print('Downsampling inputs')\n",
    "src_down, src_fpfh = preprocess_point_cloud(src, voxel_size)\n",
    "dst_down, dst_fpfh = preprocess_point_cloud(dst, voxel_size)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b5dda36a",
   "metadata": {},
   "source": [
    "## FGR配准"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "id": "f5fbc591",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Running FGR\n",
      "[Open3D DEBUG] normalize points :: mean = [43.464228 -0.602744 -0.238923]\n",
      "[Open3D DEBUG] normalize points :: mean = [0.647733 5.310233 -0.633679]\n",
      "[Open3D DEBUG] normalize points :: global scale : 267.459478\n",
      "[Open3D DEBUG] \t[cross check] \n",
      "[Open3D DEBUG] Initial matchings : 2110\n",
      "[Open3D DEBUG] \t[tuple constraint] \n",
      "[Open3D DEBUG] 20 tuples (211000 trial, 211000 actual).\n",
      "[Open3D DEBUG] \t[final] matches 60.\n",
      "[Open3D DEBUG] Pairwise rigid pose optimization\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "PointCloud with 62583 points."
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "print('Running FGR')\n",
    "result = o3d.pipelines.registration.registration_fgr_based_on_feature_matching(\n",
    "    src_down, dst_down, src_fpfh, dst_fpfh,\n",
    "    o3d.pipelines.registration.FastGlobalRegistrationOption(\n",
    "        maximum_correspondence_distance=distance_threshold,\n",
    "        iteration_number=args.max_iterations,\n",
    "        maximum_tuple_count=args.max_tuples))\n",
    "\n",
    "src.paint_uniform_color([1, 0, 0])\n",
    "dst.paint_uniform_color([0, 1, 0])\n",
    "#o3d.visualization.draw([src.transform(result.transformation), dst])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "id": "9e90e3da",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 3.01320681e-01,  9.16433548e-01,  2.63354133e-01,\n",
       "        -3.45003312e+00],\n",
       "       [ 9.53520345e-01, -2.88964944e-01, -8.54295745e-02,\n",
       "        -3.59050856e+01],\n",
       "       [-2.19041585e-03,  2.76855221e-01, -9.60909147e-01,\n",
       "         1.53145295e+00],\n",
       "       [-0.00000000e+00, -0.00000000e+00,  0.00000000e+00,\n",
       "         1.00000000e+00]])"
      ]
     },
     "execution_count": 35,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "result.transformation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "id": "f6b78f54",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[Open3D DEBUG] Add geometry and update bounding box to [(-98.6719, -35.9051, -59.3336) - (226.0496, 234.6006, 62.1785)]\n",
      "[Open3D DEBUG] Add geometry and update bounding box to [(-177.5125, -103.2817, -59.3336) - (226.0496, 234.6006, 62.1785)]\n"
     ]
    }
   ],
   "source": [
    "draw_registration_result(src,dst,result.transformation)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.16"
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
